﻿#include "OiPCH.hpp"
#include "hasp_api.hpp"
#include "hasp_vcode.hpp"

namespace Oi
{
    namespace PLScannerCore
    {

        Calib::Calib()
        {

        }

        Calib::Calib(int mode, float Sx, float Sy, float Cx, float Cy, float F, float K, float K1, float K2, float K3, float K4, float P1, float P2, float posX, float posY, float posZ, float posRx, float posRy, float posRz)
        {
            load(mode, Sx, Sy, Cx, Cy, F, K, K1, K2, K3, K4, P1, P2, posX, posY, posZ, posRx, posRy, posRz);
        }

        Calib::Calib(int mode, float Sx, float Sy, float Cx, float Cy, float F, float K, float K1, float K2, float K3, float K4, float P1, float P2, Math::Pose3d pos)
        {
            load(mode, Sx, Sy, Cx, Cy, F, K, K1, K2, K3, K4, P1, P2, pos);
        }

        Calib::Calib(const Core::Calib& cc)
        {
            load(cc);
        }

        Calib::Calib(const QString& file)
        {
            load(file);
        }

        bool Calib::isValid()
        {
            return _data._model >= 0 && _data._F > 0;
        }

        void Calib::load(Data& d)
        {
            load(d._model, d._Sx, d._Sy, d._Cx, d._Cy, d._F, d._K, d._K1, d._K2, d._K3, d._K4, d._P1, d._P2, d._posX, d._posY, d._posZ, d._posRx, d._posRy, d._posRz);
        }

        void Calib::load(int mode, float Sx, float Sy, float Cx, float Cy, float F, float K, float K1, float K2, float K3, float K4, float P1, float P2, float posX, float posY, float posZ, float posRx, float posRy, float posRz)
        {
            if(F <= 0)
            {
                return;
            }
            _data._model = mode;
            _data._Sx = Sx;
            _data._Sy = Sy;
            _data._Cx = Cx;
            _data._Cy = Cy;
            _data._F = F;
            _data._K = K;
            _data._K1 = K1;
            _data._K2 = K2;
            _data._K3 = K3;
            _data._K4 = K4;
            _data._P1 = P1;
            _data._P2 = P2;
            _data._posX = posX;
            _data._posY = posY;
            _data._posZ = posZ;
            _data._posRx = posRx;
            _data._posRy = posRy;
            _data._posRz = posRz;

            _pos = Math::Pose3d(posX, posY, posZ, posRx, posRy, posRz);
            _matrix = _pos.toMatrix();
            _invert = Math::Inverse(_matrix);
            _plane = _invert.transposed() * Math::Point4d(0, 0, 1, 0);
        }

        void Calib::load(int mode, float Sx, float Sy, float Cx, float Cy, float F, float K, float K1, float K2, float K3, float K4, float P1, float P2, Math::Pose3d pos)
        {
            if(F <= 0)
            {
                return;
            }

            _data._model = mode;
            _data._Sx = Sx;
            _data._Sy = Sy;
            _data._Cx = Cx;
            _data._Cy = Cy;
            _data._F = F;
            _data._K = K;
            _data._K1 = K1;
            _data._K2 = K2;
            _data._K3 = K3;
            _data._K4 = K4;
            _data._P1 = P1;
            _data._P2 = P2;
            _data._posX = pos.t[0];
            _data._posY = pos.t[1];
            _data._posZ = pos.t[2];
            _data._posRx = pos.r[0];
            _data._posRy = pos.r[1];
            _data._posRz = pos.r[2];
            _pos = pos;

            _matrix = _pos.toMatrix();
            _invert = Math::Inverse(_matrix);
            _plane = _invert.transposed() * Math::Point4d(0, 0, 1, 0);
        }

        void Calib::load(const Core::Calib& cc)
        {
            if (cc.F() <= 0)
            {
                return;
            }

            _data._model = cc.model();
            _data._Sx = cc.Sx();
            _data._Sy = cc.Sy();
            _data._Cx = cc.Cx();
            _data._Cy = cc.Cy();
            _data._F = cc.F();
            _data._K = cc.K();
            _data._K1 = cc.K1();
            _data._K2 = cc.K2();
            _data._K3 = cc.K3();
            _data._K4 = cc.K4();
            _data._P1 = cc.P1();
            _data._P2 = cc.P2();

            _pos = cc.poses()[cc.worldPoseIndex()];

            _data._posX = _pos.t[0];
            _data._posY = _pos.t[1];
            _data._posZ = _pos.t[2];
            _data._posRx = _pos.r[0];
            _data._posRy = _pos.r[1];
            _data._posRz = _pos.r[2];


            _matrix = _pos.toMatrix();
            _invert = Math::Inverse(_matrix);
            _plane = _invert.transposed() * Math::Point4d(0, 0, 1, 0);
        }

        void Calib::load(const QString& file)
        {
            if (file == "<Dongle>")
            {
                if (OiCheckDongle())
                {
                    if(!readDataFromHasp())
                    {

                    }
                }
            }
            else
            {
                Core::Calib cc(file);
                load(cc);
            }
        }

        Math::Point2d Calib::toWorldHelper(const Math::Point2d& point) const
        {
            double uu = ( point.x() - _data._Cx ) * _data._Sx;
            double vv = ( point.y() - _data._Cy ) * _data._Sy;

            double u = 0.0;
            double v = 0.0;

            double r = uu * uu + vv * vv;

            switch(_data._model)
            {
            case 0: //径向
            {
                u = uu / ( 1.0 + _data._K * r );
                v = vv / ( 1.0 + _data._K * r );
                u /= _data._F;
                v /= _data._F;
            }
            break;
            case 1: //切向
            {
                double c = _data._K1 * r + _data._K2 * r * r + _data._K3 * r * r * r;
                u = uu + uu * c + _data._P1 * ( r + 2.0 * uu * uu ) + 2.0 * _data._P2 * uu * vv;
                v = vv + vv * c + _data._P2 * ( r + 2.0 * vv * vv ) + 2.0 * _data._P1 * uu * vv;
                u /= _data._F;
                v /= _data._F;
            }
            break;
            }

            return Math::Point2d(u, v);
        }

        Math::Point2d Calib::toImageHelper(const Math::Point2d& point) const
        {
            double u = point.x() * _data._F;
            double v = point.y() * _data._F;

            double x = 2.0 * u / ( 1.0 + Math::Sqrt(1.0 - 4 * _data._K * ( u * u + v * v )) );
            double y = 2.0 * v / ( 1.0 + Math::Sqrt(1.0 - 4 * _data._K * ( u * u + v * v )) );

            x = x / _data._Sx + _data._Cx;
            y = y / _data._Sy + _data._Cy;

            return Math::Point2d(x, y);
        }

        Math::Point3d Calib::toWorld(const Math::Point2d& point) const
        {
            if(_data._model < 0 || Math::IsNan(point.x()) || Math::IsNan(point.y()))
            {
                return Math::Point3d(NaN, NaN, NaN);
            }

            Math::Point2d cp = point;
            if(PLScanner::instance()->mirrorCameraX)
            {
                cp.x() = 1280 - cp.x();
            }

            if(PLScanner::instance()->mirrorCameraY)
            {
                cp.y() = 1024 - cp.y();
            }

            Math::Point2d sample = toWorldHelper(cp);

            double z = -1.0 * _plane[3] / ( _plane[0] * sample.x() + _plane[1] * sample.y() + _plane[2] );

            auto temp = _invert * Math::Point3d(sample.x() * z, sample.y() * z, z);

            double flipX = PLScanner::instance()->flipX ? -1.0 : 1.0;
            double flipY = PLScanner::instance()->flipY ? -1.0 : 1.0;

            return Math::Point3d(flipX * temp.x(), 0, flipY * temp.y());
        }

        Core::PointMapPtr Calib::toWorld(const Core::RangeMapPtr& rangeMap, int offsetX/* = 0*/, int offsetY /*= 0*/) const
        {
            auto pointMap = Core::PointMapPtr::create(rangeMap->cols(), rangeMap->count());

            for(int i = 0; i < rangeMap->count(); i++)
            {
                float* x = pointMap->xLine(i);
                float* y = pointMap->yLine(i);
                float* z = pointMap->zLine(i);

                float* ptr = rangeMap->profile(i);

                for(int j = 0; j < rangeMap->cols(); j++)
                {
                    if(ptr[j] > 0)
                    {
                        auto worldPoint = toWorld(Math::Point2d(j + offsetX, ptr[j] + offsetY));
                        x[j] = worldPoint.x();
                        y[j] = worldPoint.y();
                        z[j] = worldPoint.z();
                    }
                    else
                    {
                        x[j] = NaN;
                        y[j] = NaN;
                        z[j] = NaN;
                    }
                }
            }

            pointMap->resize(rangeMap->count());
            return pointMap;
        }

        Math::Point2d Calib::toImage(const Math::Point2d& world) const
        {
            auto x = PLScanner::instance()->flipX ? -world.x() : world.x();
            auto y = PLScanner::instance()->flipY ? -world.y() : world.y();

            Math::Point3d p =  Math::Point3d(x, y, 0);

            Math::Point3d entry = _matrix * p;

            auto node = toImageHelper(Math::Point2d(entry.x() / entry.z(), entry.y() / entry.z()));

            if(PLScanner::instance()->mirrorCameraX)
            {
                node.x() = 1280 - node.x();
            }

            if(PLScanner::instance()->mirrorCameraY)
            {
                node.y() = 1024 - node.y();
            }

            return Math::Point2d(node.x(), node.y());
        }

        bool Calib::saveDataToHasp()
        {
            Hasp_Api::hasp_status_t   status;
            Hasp_Api::hasp_handle_t   handle;

            status =  Hasp_Api::hasp_login(HASP_DEFAULT_FID,
                (Hasp_Api::hasp_vendor_code_t)vendor_code,
                &handle);

            if (status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            Hasp_Api::hasp_size_t size;

            status = Hasp_Api::hasp_get_size(handle,
                HASP_FILEID_RW,
                &size);
            
            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            if(size < sizeof(Data) + seriesSize)
            {
                return false;
            }

            status = Hasp_Api::hasp_write(handle,
                HASP_FILEID_RW,
                seriesSize,
                sizeof(_data),
                (void*)&_data);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            status = Hasp_Api::hasp_logout(handle);

            return true;
        }

        bool Calib::readDataFromHasp()
        {
            Hasp_Api::hasp_status_t   status;
            Hasp_Api::hasp_handle_t   handle;

            status = Hasp_Api::hasp_login(HASP_DEFAULT_FID,
                ( Hasp_Api::hasp_vendor_code_t )vendor_code,
                &handle);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            Hasp_Api::hasp_size_t size;

            status = Hasp_Api::hasp_get_size(handle,
                HASP_FILEID_RW,
                &size);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            if(size < sizeof(Data) + seriesSize)
            {
                return false;
            }

            Data data;

            status = Hasp_Api::hasp_read(handle,
                HASP_FILEID_RW,
                seriesSize,
                sizeof(data),
                &data);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            load(data);
            
            status = Hasp_Api::hasp_logout(handle);

            return true;
        }

        bool Calib::saveSeriesToHasp(const std::string s)
        {
            Hasp_Api::hasp_status_t   status;
            Hasp_Api::hasp_handle_t   handle;

            status = Hasp_Api::hasp_login(HASP_DEFAULT_FID,
                ( Hasp_Api::hasp_vendor_code_t )vendor_code,
                &handle);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            Hasp_Api::hasp_size_t size;

            status = Hasp_Api::hasp_get_size(handle,
                HASP_FILEID_RW,
                &size);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            if(size < seriesSize)
            {
                return false;
            }

            status = Hasp_Api::hasp_write(handle,
                HASP_FILEID_RW,
                0,
                seriesSize,
                s.c_str());

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            status = Hasp_Api::hasp_logout(handle);

            return true;
        }

        bool Calib::readSeriesFromHasp(std::string& s)
        {
            Hasp_Api::hasp_status_t   status;
            Hasp_Api::hasp_handle_t   handle;

            status = Hasp_Api::hasp_login(HASP_DEFAULT_FID,
                ( Hasp_Api::hasp_vendor_code_t )vendor_code,
                &handle);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            Hasp_Api::hasp_size_t size;

            status = Hasp_Api::hasp_get_size(handle,
                HASP_FILEID_RW,
                &size);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            if(size < seriesSize)
            {
                return false;
            }

            char d[13];

            status = Hasp_Api::hasp_read(handle,
                HASP_FILEID_RW,
                0,
                seriesSize,
                d);

            if(status != Hasp_Api::HASP_STATUS_OK)
            {
                return false;
            }

            d[12] = '\0';

            s = d;

            status = Hasp_Api::hasp_logout(handle);

            return true;
        }

        Calib* Calib::OiCalib()
        {
            static Calib s_calib;
            return &s_calib;
        }

        PosCalib::PosCalib()
        {
            _data._K1 = Core::OiSettings()->value("Calib/PosK1", 0.0).toDouble();
            _data._K2 = Core::OiSettings()->value("Calib/PosK2", 1.0).toDouble();
            _data._K3 = Core::OiSettings()->value("Calib/PosK3", 0.0).toDouble();
        }

        PosCalib::PosCalib(const PosCalib& p)
        {
            *this = p;
        }

        double PosCalib::toPos(QPointF camCoord)
        {
            return _data._K1 * camCoord.x() + _data._K2 * camCoord.y() + _data._K3;
        }

        double PosCalib::toCam(double pos, double camX)
        {
            return ( pos - _data._K1 * camX - _data._K3 ) / _data._K2;
        }

        bool PosCalib::calib(QList<double> xPos, QList<double> yPos, QList<double> actPos)
        {
            QList<double> errors;

            if (xPos.size() < 3)
            {
                return false;
            }

            if (xPos.size() != yPos.size() || xPos.size() != actPos.size())
            {
                return false;
            }

            cv::Mat_<float> matA(xPos.size(), 3);
            cv::Mat_<float> matB(xPos.size(), 1);
            cv::Mat_<float> matX(3, 1);

            for (int i = 0; i < xPos.size(); i++)
            {
                matA(i, 0) = xPos[i];
                matA(i, 1) = yPos[i];
                matA(i, 2) = 1;
                matB(i, 0) = actPos[i];
            }

            if(!cv::solve(matA, matB, matX, CV_SVD))
            {
                return false;
            }

            _data._K1 = matX(0, 0);

            _data._K2 = matX(1, 0);

            _data._K3 = matX(2, 0);

            for (int i = 0; i < xPos.size(); i++)
            {
                errors.push_back( _data._K1 * xPos[i] + _data._K2 * yPos[i] + _data._K3 - actPos[i]);
            }

            OiWarning() <<"Error:"<< errors;

            Core::OiSettings()->setValue("Calib/PosK1", (double)_data._K1);
            Core::OiSettings()->setValue("Calib/PosK2", (double)_data._K2);
            Core::OiSettings()->setValue("Calib/PosK3", (double)_data._K3);
            Core::OiSettings()->sync();

            return true;
        }

        PosCalib& PosCalib::operator=(const PosCalib& p)
        {
            _data._K1 = p._data._K1;
            _data._K2 = p._data._K2;
            _data._K3 = p._data._K3;

            return *this;
        }

        PosCalib* PosCalib::OiPosCalib()
        {
            static PosCalib s_calib;
            return &s_calib;
        }

    }
}